/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2023-02-24     86188       the first version
 */
#include "head_rtthread.h"
#include "head_user.h"


Sensor_HandleTypedef Sensor_Handle;
/*
***********************************************************************
* model name: MS5611 driver
* brief:
*
*   copyright(C), 2020-2099, www.xxx.com
***********************************************************************
*/
static void MS5611_R_reg(uint8_t addr, uint8_t *buf, uint32_t num)
{
    uint8_t dat;

    dat =  addr;
    SPI_Handle.WthenRs(SPI_Handle.SPI1dev.MS5611, &dat, 1, buf, num);
}
static void MS5611_Write(uint8_t val)
{
    SPI_Handle.RWBytes(SPI_Handle.SPI1dev.MS5611, &val, RT_NULL, 1);
}
static void delay_ms(uint16_t ms)
{
    rt_thread_delay(ms);
}

static void MS5611_Reset(void)
{
    MS5611_Write(CMD_RESET);
}
/****************************************************************************
*@brief     measure temperature
*****************************************************************************/
static void MS5611_Start_T(void)
{
    MS5611_Write(CMD_ADC_CONV | CMD_ADC_D2 | MS5611_OSR); //D2 (temperature) conversion start!
}

/*****************************************************************************
*@brief     read raw_ADC data of temperature
*****************************************************************************/
static uint8_t temper_dat[3];
static void MS5611_Read_Adc_T(void)
{
    MS5611_R_reg(CMD_ADC_READ, temper_dat, 3);
}

/*****************************************************************************
*@brief     measure automospheric pressure
*****************************************************************************/
static void MS5611_Start_P(void)
{
    MS5611_Write(CMD_ADC_CONV | CMD_ADC_D1 | MS5611_OSR); //D1 (pressure) conversion start!
}

/*****************************************************************************
*@brief  read raw_ADC data of automospheric pressure
*****************************************************************************/
static uint8_t pressure_dat[3];
static void MS5611_Read_Adc_P(void)
{
    MS5611_R_reg(CMD_ADC_READ, pressure_dat, 3);
}

/****************************************************************************************
*@brief   read calibration parameters of temperature and pressure
*****************************************************************************************/
static uint16_t MS5611_CaliProm[PROM_NB];   //parameter is located on-chip ROM
static void MS5611_Read_Prom(void)
{
    for (uint8_t i = 0; i < PROM_NB; i++)
    {
        MS5611_R_reg(CMD_PROM_RD+i*2, (uint8_t*)(MS5611_CaliProm+i), 2);
    }
    MS5611_CaliProm[1] = MS5611_CaliProm[1]<<8 | MS5611_CaliProm[1]>>8;
    MS5611_CaliProm[2] = MS5611_CaliProm[2]<<8 | MS5611_CaliProm[2]>>8;
    MS5611_CaliProm[3] = MS5611_CaliProm[3]<<8 | MS5611_CaliProm[3]>>8;
    MS5611_CaliProm[4] = MS5611_CaliProm[4]<<8 | MS5611_CaliProm[4]>>8;
    MS5611_CaliProm[5] = MS5611_CaliProm[5]<<8 | MS5611_CaliProm[5]>>8;
    MS5611_CaliProm[6] = MS5611_CaliProm[6]<<8 | MS5611_CaliProm[6]>>8;
}

/*****************************************************************************
*@brief  MS5611-Init
*****************************************************************************/
static void MS5611_Init(void)
{
    MS5611_Reset();
    delay_ms(10);
    MS5611_Read_Prom();
    delay_ms(10);
    MS5611_Start_T();
}

/*****************************************************************************
*@brief  calculate automospheric pressure
*@brief  密集型计算，计算方式需要细致考虑
*****************************************************************************/
static void MS5611_BaroCalculate(float* pre, float* tem)
{
    int64_t  OFF = 0, SENS = 0, OFF2 = 0, SENS2 = 0, T2 = 0;
    uint32_t D1 = 0, D2 = 0;
    int32_t  dT = 0, TEMP = 0, P = 0;

    /* raw 24bits ADC */
    D1 = (uint32_t)pressure_dat[0]<<16 | (uint32_t)pressure_dat[1]<<8 | (uint32_t)pressure_dat[2]<<0;
    D2 = (uint32_t)temper_dat[0]  <<16 | (uint32_t)temper_dat[1]  <<8 | (uint32_t)temper_dat[2]  <<0;

    /* dT = D2 - C5*2^8 */
    dT = D2 - ((uint32_t)MS5611_CaliProm[5]*256);
    /* TEMP = 2000 + dT*C6/2^23 */
    TEMP = 2000 + (((int64_t)dT*MS5611_CaliProm[6])/8388608);

    /* OFF =  C2*2^16 + (dT*C4)/2^7 */
    OFF = ((int64_t)MS5611_CaliProm[2]*65536) + (((int64_t)dT*MS5611_CaliProm[4])/128);
    /* SENS = C1*2^15 + (dT*C3)/2^8 */
    SENS = ((int64_t)MS5611_CaliProm[1]*32768) + (((int64_t)dT*MS5611_CaliProm[3])/256);

    /* temperature compensation */
    if(TEMP < 2000)          //temperature lower than 20
    {
        T2    = dT * dT;
        OFF2  = 5*(TEMP-2000)*(TEMP-2000)/2;
        SENS2 = 5*(TEMP-2000)*(TEMP-2000)/4;
        if(TEMP < -1500)     //temperature lower than -15
        {
            OFF2  +=  7*(TEMP+1500)*(TEMP+1500);
            SENS2 += 11*(TEMP+1500)*(TEMP+1500)/2;
        }
    }

    TEMP  -= T2;
    OFF   -= OFF2;
    SENS  -= SENS2;

    /* P = (D1*SENS/2^21 - OFF)/2^15 */
    P = (D1*SENS/2097152 - OFF)/32768;
    /* result */
    *tem = (float)TEMP/100.0f;
    *pre = (float)P/100.0f;
}

/*****************************************************************************
*@brief  read and get measure
*@param[in] pre: automospheric pressure
*@param[in] tem: temperature
*****************************************************************************/
#include "math.h"
static float Ms5611_pressure, Ms5611_temperature, Height;
static float Baro_to_height(float baro, float temper)
{
    return (powf(1013.25f/Ms5611_pressure, 1/5.257f)-1)*(Ms5611_temperature+273.15f)/0.0065f*100.0f;
    //return (101325.0f - Ms5611_pressure*100.0f)*9.0f;
}
static void MS5611_Update(void)
{
    static uint8_t state = 0;
    if(state)
    {
        MS5611_Read_Adc_P();
        MS5611_Start_T();
        MS5611_BaroCalculate(&Ms5611_pressure, &Ms5611_temperature);
        Height = Baro_to_height(Ms5611_pressure, Ms5611_temperature);
        state = 0;
    }
    else
    {
        MS5611_Read_Adc_T();
        MS5611_Start_P();
        state = 1;
    }
}

static int rt_ms5611_init(void)
{
    MS5611_Init();

    Sensor_Handle.height     = &Height;
    Sensor_Handle.BaroUpdate = MS5611_Update;

    return RT_EOK;
}

INIT_DEVICE_EXPORT(rt_ms5611_init);

